#include "setFormation.h"
#include "Seek.h"
#include "Arrive.h"
#include "Face.h"
#include "Separation.h"
#include "../Steering.h"
#include "../Character.h"
#include "../Obstacle.h"

setFormation::setFormation(Character * _character,Character* _player,vector<Character*>* ch,vector<Obstacle*>* ob){
  player = _player;
  character = _character;
  //-  Aqui introducimos el comportamiento Seek en la 1era. posicion -//
  Seek * seek = new Seek();
  seek->SetCharacter(_character);
  seek->SetTarget(_player);
  set.push_back(seek);
  //-  Aqui introducimos el comportamiento Face en la 2da. posicion -//
  Face * face = new Face();
  face->SetCharacter(_character);
  face->SetTarget(_player);
  set.push_back(face);
  //-  Aqui introducimos el comportamiento Separation en la 3era. posicion -//
  Separation * sep = new Separation(ch,ob);
  sep->SetCharacter(_character);
  sep->SetTarget(_player);
  set.push_back(sep);
}

Steering* setFormation::getSteering(){
  Steering* ret = new Steering();
  float* aux;
  aux = asVector(player->getKinematic()->orientation);
  aux[0] = -1*aux[0]*70;
  aux[1] = -1*aux[1]*70;
  sumarVectores(aux,player->getKinematic()->position,aux);
  Kinematic* k = new Kinematic();
  k->position[0]= aux[0];
  k->position[1]=aux[1];
  Character*  c = new Character(k);
//cout << " vector " << aux[0] << ","<< aux[1]<< "," << aux[2] << endl;
  set[0]->SetTarget(c);
  for(int i = 0 , n = set.size()-1 ; i < n ; i++){
    Steering* auxS = set[i]->getSteering();
    sumarVectores(ret->linear,auxS->linear,ret->linear);
    ret->angular+=auxS->angular;
  }
  Steering* auxS = set[set.size()-1]->getSteering();
  multVectorScalar(auxS->linear,0.1);
  sumarVectores(ret->linear,auxS->linear,ret->linear);
  ret->angular+=auxS->angular;
  return ret;
}
